#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "hardware/claim.h"
#include <string.h>
#include "hardware.h"
#include "i2c.h"
#include "arduino.h"
#include "log.h"

#define I2C_HW          &i2c0_inst
static i2c_client       *g_i2c_head;
static int i2c_running = 0;

void i2c_add(i2c_client *client, void (*setup)(void), void (*loop)(void))
{
    client->setup = setup;
    client->loop = loop;
    client->next = g_i2c_head;
    g_i2c_head = client;
}

bool reserved_addr(uint8_t addr) {
    return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
}

void i2c_scan(void)
{
    log_printf("\nI2C Bus Scan\n");
    log_printf_raw("   0  1  2  3  4  5  6  7  8  9  A  B  C  D  E  F\n");
 
    for (int addr = 0; addr < (1 << 7); ++addr) {
        if (addr % 16 == 0) {
            log_printf_raw("%02x ", addr);
        }
 
        // Perform a 1-byte dummy read from the probe address. If a slave
        // acknowledges this address, the function returns the number of bytes
        // transferred. If the address byte is ignored, the function returns
        // -1.
 
        // Skip over any reserved addresses.
        int ret;
        uint8_t rxdata;
        
        if (reserved_addr(addr)) {
            ret = PICO_ERROR_GENERIC;
        } else {
            uint32_t save = hw_claim_lock();
            ret = i2c_read_blocking(i2c_default, addr, &rxdata, 1, false);
            hw_claim_unlock(save);
        }
        log_printf_raw(ret < 0 ? "." : "@");
        log_printf_raw(addr % 16 == 15 ? "\n" : "  ");
    }
    log_printf("Done.\n");
}

void i2c_init_gpio(void)
{
    i2c_init(I2C_HW, 400 * 1000);
    gpio_pull_up(PIN_OUT_PWM_SCL);
    gpio_pull_up(PIN_OUT_PWM_SDA);

    gpio_set_function(PIN_OUT_PWM_SCL, GPIO_FUNC_I2C);
    gpio_set_function(PIN_OUT_PWM_SDA, GPIO_FUNC_I2C);
    i2c_scan();
    i2c_running = 1;
}

void i2c_setup(void) 
{ 
    i2c_client *client = g_i2c_head;

    i2c_init_gpio();
    while (client) {
        client->setup();
        client = client->next;
    }
}

void i2c_loop(void) 
{
    i2c_client *client = g_i2c_head;

    while (client) {
        client->loop();
        client = client->next;
    }
}

ARDUINO_CLIENT_DEFINE(i2c_setup, i2c_loop);

void __no_inline_not_in_flash_func(i2c_write_reg)(uint8_t addr, uint8_t reg, uint8_t val)
{
    uint8_t buf[2] = {reg, val};

    if (i2c_running == 0) {
        return;
    }
    uint32_t save = hw_claim_lock();
    i2c_write_blocking(I2C_HW, addr, buf, 2, false);
    hw_claim_unlock(save);
}

uint8_t i2c_read_reg(uint8_t addr, uint8_t reg)
{
    uint8_t data = 0;

    if (i2c_running == 0) {
        return 0;
    }
    uint32_t save = hw_claim_lock();
    i2c_write_blocking(I2C_HW, addr, &reg, 1, true);
    i2c_read_blocking(I2C_HW, addr, &data, 1, false);
    hw_claim_unlock(save);
    return data;
}

void i2c_pause(void)
{
    i2c_running = 0;
}

void i2c_resume(void)
{
    i2c_running = 1;
}